/**
 * @file auto_driving_scheduler.cpp
 * @brief 自动驾驶调度器
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-05-03
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */

#include <thread>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <ros_openvino/ObjectBoxList.h>

#include <dynamic_reconfigure/client.h>
#include <lane_detector/tutorialsConfig.h>

using std::vector;

void (*task)(void);

class Flag{
public:
    bool is_there_a_person = false;
    bool is_there_a_obstacle = false;
    bool is_there_a_stop_sign = false;
    bool is_there_a_limit_sign = false;
    bool is_there_a_parking_sign = false;
    bool is_there_a_do_not_enter_sign = false;

    bool is_hign_speed = true; // 是否处于高速状态，用于处理限速标志

    ros::Time detect_person_stamp; // 时间戳，检测到目标时进行更新，如果一段时间没有检测到目标，则将相应的标志量置为false
    ros::Time detect_stop_sign_stamp;
    ros::Time detect_limit_sign_stamp;
    ros::Time detect_parking_sign_stamp;
    ros::Time detect_do_not_enter_sign_stamp;

    Flag(){
        detect_person_stamp = ros::Time::now();
        detect_stop_sign_stamp = ros::Time::now();
        detect_limit_sign_stamp = ros::Time::now();
        detect_parking_sign_stamp = ros::Time::now();
        detect_do_not_enter_sign_stamp = ros::Time::now();
    }
};
// void 

bool new_lane_cmd_flag = false;
bool new_object_flag   = false;
geometry_msgs::Pose  pose;
geometry_msgs::Point cross_position_1; // 路口位置
geometry_msgs::Point cross_position_2; // 路口位置
geometry_msgs::Point cross_position;   // 路口中心位置
            
geometry_msgs::Point parking_lot_position;


double GetDistance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2){
    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
}

bool MoveServer(const geometry_msgs::Point& p1, 
                const geometry_msgs::Point& p2, 
                double theta_curr,
                double theta_dest,
                double t,
                geometry_msgs::Twist& cmd_vel)
{
    static int state = 0;
    static auto start = ros::Time::now();

    static double v = 0; // 线速度
    static double omega = 0; // 角速度
    if(state == 0)
    {
        double delta_theta = theta_dest - theta_curr;
        omega = delta_theta / t; // 角速度
        if(p1 == p2)
        {
            v = 0;
        }
        else
        {
            // double dist = GetDistance(p1, p2);
            // double l = delta_theta * dist / 2 / sin(delta_theta / 2);
            // v = l / t; // 线速度
            double r = GetDistance(p1, p2);
            v = fabs(delta_theta * r / t);
        }
        state++;
        start = ros::Time::now();
    }
    if(state == 1)
    {
        cmd_vel.linear.x = v;
        cmd_vel.angular.z = omega;
        if(ros::Time::now().toSec() - start.toSec() >= t)
        {
            start = ros::Time::now();
            state = 2;
            // return true;
        }
    }
    else if(state == 2) // 前进一段距离，使巡线输出稳定
    {
        cmd_vel.linear.x = v;
        cmd_vel.angular.z  = 0;
        if(ros::Time::now().toSec() - start.toSec() >= 1)
        {
            state = 0;
            return true;
        }
    }
    return false;
}

void GetCmd(geometry_msgs::Twist& cmd_vel, 
            const std::vector<ros_openvino::ObjectBox>& object_list,
            const std::string& label)
{
    std::for_each(object_list.begin(), object_list.end(), [&](ros_openvino::ObjectBox obj){
        if(obj.label == label){
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = 0;
        }
    });
}
/**
 * 标签表
 * parking
 * stop
 * person
 * speedLimit
 * trafficLight
 */
void UpdataFlag(ros_openvino::ObjectBoxList& object_list, double min_dist, Flag& flag){
    // 障碍物检测
    if(min_dist < 0.4)
        flag.is_there_a_obstacle = true;
    else 
        flag.is_there_a_obstacle = false;

    if(ros::Time::now().toSec() - object_list.header.stamp.toSec() < 0.2)
    {
        std::sort(object_list.objectboxes.begin(), object_list.objectboxes.end(), [&](ros_openvino::ObjectBox& obj1, ros_openvino::ObjectBox& obj2){
            return obj1.confidence > obj2.confidence;            
        });
        // 行人检测
        if(object_list.objectboxes[0].label == "person"){
            flag.is_there_a_person = true;
            flag.detect_person_stamp = ros::Time::now();
        }
        // 禁行标志
        if(object_list.objectboxes[0].label == "stop"){
            flag.is_there_a_stop_sign = true;
            flag.detect_stop_sign_stamp = ros::Time::now();
        }
        // 限速标志
        if(object_list.objectboxes[0].label == "speedLimit"){
            flag.is_there_a_limit_sign = true;
            flag.detect_limit_sign_stamp = ros::Time::now();
        }
        // 停车标志
        if(object_list.objectboxes[0].label == "parking"){
            flag.is_there_a_parking_sign = true;
            flag.detect_parking_sign_stamp = ros::Time::now();
        }
    }
    else{
        auto now = ros::Time::now();

        if(now.toSec() - flag.detect_parking_sign_stamp.toSec() > 2)
            flag.is_there_a_parking_sign = false; 
        if(now.toSec() - flag.detect_do_not_enter_sign_stamp.toSec() > 2)
            flag.is_there_a_do_not_enter_sign = false; 
        if(now.toSec() - flag.detect_limit_sign_stamp.toSec() > 4)
            flag.is_there_a_limit_sign = false; 
        if(now.toSec() - flag.detect_stop_sign_stamp.toSec() > 2)
            flag.is_there_a_stop_sign = false; 
        if(now.toSec() - flag.detect_person_stamp.toSec() > 2)
            flag.is_there_a_person = false; 
    }

}

int main(int argc, char** argv){

    ros::init(argc, argv, "auto_driving_scheduler");
    ros::NodeHandle n;

    ros::Publisher cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);

    ros::NodeHandle private_nh("~");

    private_nh.param("cross_position_1_x", cross_position_1.x, 0.0);
    private_nh.param("cross_position_1_y", cross_position_1.y, 0.0);

    private_nh.param("cross_position_2_x", cross_position_2.x, 0.0);
    private_nh.param("cross_position_2_y", cross_position_2.y, 0.0);

    private_nh.param("cross_position_x", cross_position.x, 0.0);
    private_nh.param("cross_position_y", cross_position.y, 0.0);

    private_nh.param("parking_lot_position_x", parking_lot_position.x, 0.0);
    private_nh.param("parking_lot_position_y", parking_lot_position.y, 0.0);

    //订阅服务，并设置回调函数用于回读改变后的最新参数
    dynamic_reconfigure::Client<lane_detector::tutorialsConfig> client("lane_detect_node", [&](const lane_detector::tutorialsConfig config){});
    lane_detector::tutorialsConfig config;
    // config.linear_x = 1;
    // config.angle_ctrl_coeffi = 0.3;
    // client.setConfiguration(config);

    ros::Rate loop_rate(30);

    // 订阅车道线检测结果
    geometry_msgs::Twist twist;
    ros::Subscriber lane_cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("/lane_detect_node/cmd_vel", 1, [&](const geometry_msgs::Twist::ConstPtr msgs){
                        twist.linear.x      = msgs->linear.x;
                        twist.angular.z     = msgs->angular.z;    
                        new_lane_cmd_flag   = true;                    
                    });


    // 订阅目标检测结果
    
    ros_openvino::ObjectBoxList objects_list;
    auto obj_list_stamp = ros::Time::now();
    ros::Subscriber ObjectBoxList = n.subscribe<ros_openvino::ObjectBoxList>("/object_detection/box_list", 1, [&](const ros_openvino::ObjectBoxList::ConstPtr msgs){
                        // std::vector<ros_openvino::ObjectBox> object_list;
                        if(!msgs->objectboxes.empty()) {
                            // 更新时间戳
                            obj_list_stamp = ros::Time::now();
                            objects_list.objectboxes.clear();
                            objects_list.header.stamp = ros::Time::now();
                        }
                        std::for_each(msgs->objectboxes.begin(), msgs->objectboxes.end(), [&](ros_openvino::ObjectBox obj){
                            // if(obj.label == "person"){
                                objects_list.objectboxes.push_back(obj);                                
                                new_object_flag = true;
                            // }
                        });

                        
                    });

    // 订阅激光雷达测距结果
    double laser_min_dist = 0;
    ros::Subscriber laser_dist_sub = n.subscribe<geometry_msgs::Point>("/laser_node/min_dist", 1, [&laser_min_dist](const geometry_msgs::Point::ConstPtr msgs){
                        laser_min_dist = msgs->z;
                        // ROS_INFO("min_dist: %f", laser_min_dist);
                    });
                    
    // 监听tf
    tf::TransformListener listener;
    tf::StampedTransform transform;
    std::thread t([&](){ 
        while(true){
            try
            {
                // 监听从base_link到map的转换
                listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(3.0));
                listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
            }
            catch(tf::TransformException &ex)
            {
                ROS_ERROR("%s", ex.what());
                ros::Duration(1.0).sleep();
                continue;
            }
            pose.position.x = transform.getOrigin().x();
            pose.position.y = transform.getOrigin().y();
            pose.orientation.x = transform.getRotation().x();
            pose.orientation.y = transform.getRotation().y();
            pose.orientation.z = transform.getRotation().z();
            pose.orientation.w = transform.getRotation().w();

            ROS_INFO("%f, %f, %f", pose.position.x, pose.position.y, tf::getYaw(pose.orientation) * 180 / 3.1415926535);
            loop_rate.sleep();
        }
        });
    t.detach();

    Flag flag;

    ///////////////////////////////////////////////////////////////////
    ////////////////////////// 循环  //////////////////////////////////
    //////////////////////////////////////////////////////////////////

    while(ros::ok()){

        // 更新标志量
        UpdataFlag(objects_list, laser_min_dist, flag);

        static int state = 0;
        static int last_state = 0;
        ROS_INFO("---------------------------");
        ROS_INFO("state   : %d", state);
        ROS_INFO("obstacle: %d", flag.is_there_a_obstacle);
        ROS_INFO("person  : %d", flag.is_there_a_person);
        ROS_INFO("limit   : %d", flag.is_there_a_limit_sign);
        ROS_INFO("parking : %d", flag.is_there_a_parking_sign);
        ROS_INFO("stop    : %d", flag.is_there_a_stop_sign);

        if(state == 0)
        {
            // 禁行标志处理
            // if(flag.is_there_a_limit_sign)
            // {
            //     if(flag.is_hign_speed)
            //     {
            //         config.linear_x = 0.15;
            //         config.angle_ctrl_coeffi = 0.002;
            //         client.setConfiguration(config);
            //         flag.is_hign_speed = false;
            //     }
            // }
            // else
            // {
            //     if(!flag.is_hign_speed)
            //     {
            //         config.linear_x = 0.25;
            //         config.angle_ctrl_coeffi = 0.003;
            //         client.setConfiguration(config);
            //         flag.is_hign_speed = true;
            //     }
            // }

            if(flag.is_there_a_obstacle || flag.is_there_a_person)
            {
                // 有障碍物或者有行人，直接停止
                twist.angular.z = 0;
                twist.linear.x = 0;
                cmd_pub.publish(twist);                
            }
            else if(new_lane_cmd_flag)
            {
                // 否则继续巡线
                cmd_pub.publish(twist);
                new_lane_cmd_flag = false;
            }

            if(flag.is_there_a_stop_sign)
            {
                last_state = state;
                state = 3;
            }

            if(flag.is_there_a_limit_sign)
            {
                last_state = state;
                state = 4;
            }


            if(GetDistance(pose.position, cross_position_2) < 0.2
                && tf::getYaw(pose.orientation) > 0.785
                && tf::getYaw(pose.orientation) < 2.355)
            {
                ROS_INFO("get cross 2!");
                last_state = state;
                state = 1;
            }

            if(GetDistance(pose.position, cross_position_1) < 0.2
                && (tf::getYaw(pose.orientation) > 2.355
                || tf::getYaw(pose.orientation) < -2.355))
            {
                ROS_INFO("get cross 1!");
                last_state = state;
                state = 2;
            }
        }
        else if(state == 1) // 从位置2转向到位置1
        {
            geometry_msgs::Twist cmd_vel;
            if(MoveServer(pose.position, cross_position, 1.80, 0, 3, cmd_vel))
            // if(MoveServer(pose.position, cross_position, tf::getYaw(pose.orientation), 0, 3, cmd_vel))
            // if(MoveServer(pose.position, cross_position_1, 1.57, 0, 2, cmd_vel))
            {
                state = last_state;
            }
            cmd_pub.publish(cmd_vel);
        }
        else if(state == 2) // 从位置1转向到位置2
        {
            geometry_msgs::Twist cmd_vel;
            if(MoveServer(pose.position, cross_position, 0, 1.57, 3, cmd_vel))
            // if(MoveServer(pose.position, cross_position_2, 0, 1.57, 2, cmd_vel))
            {
                state = last_state;
            }
            cmd_pub.publish(cmd_vel);
        }
        else if(state == 3) // 180度转向
        {
            geometry_msgs::Twist cmd_vel;
            if(MoveServer(pose.position, pose.position, 3.30, 0, 2, cmd_vel))
            {
                state = last_state;
            }
            cmd_pub.publish(cmd_vel);
        }
        else if(state == 4) // 停车
        {
            static int _state = -1;
            static auto start_stamp = ros::Time::now();
            static double omega = 0;
            static double v = 0;
            if(_state == -1){
                double delta_theta = 0 - tf::getYaw(pose.orientation);
                omega = delta_theta / 2; // 角速度
                double dist = GetDistance(pose.position, parking_lot_position);
                v = dist / 2; // 线速度
                _state++;
                start_stamp = ros::Time::now();
            }
            if(_state == 0){
                geometry_msgs::Twist cmd_vel;
                if(ros::Time::now().toSec() - start_stamp.toSec() < 2)
                {
                    cmd_vel.angular.z = omega;
                    cmd_vel.linear.x = v;
                }
                else
                {
                    _state++;
                    start_stamp = ros::Time::now();
                }
                cmd_pub.publish(cmd_vel);
            }
            else if(_state == 1){ // 进入停车场
                geometry_msgs::Twist cmd_vel;
                if(ros::Time::now().toSec() - start_stamp.toSec() < 2)
                {
                    cmd_vel.linear.x = 0;
                    cmd_vel.linear.y = 0.2;
                    cmd_vel.angular.z = 0;
                }
                else
                {
                    cmd_vel.linear.x = 0;
                    cmd_vel.linear.y = 0;
                    cmd_vel.angular.z = 0;
                    _state++;
                    start_stamp = ros::Time::now();
                    // state = last_state;
                }
                cmd_pub.publish(cmd_vel);
            }
            else if(_state == 2)
            {
                if(ros::Time::now().toSec() - start_stamp.toSec() > 1)
                {
                    _state++;
                    start_stamp = ros::Time::now();
                }
            }
            else if(_state == 3){ // 从停车位出来
                geometry_msgs::Twist cmd_vel;
                if(ros::Time::now().toSec() - start_stamp.toSec() < 1.6)
                {
                    cmd_vel.linear.x = 0;
                    cmd_vel.linear.y = -0.18;
                    cmd_vel.angular.z = 0;
                }
                else
                {
                    cmd_vel.linear.x = 0;
                    cmd_vel.linear.y = 0;
                    cmd_vel.angular.z = 0;
                    _state++;
                    start_stamp = ros::Time::now();
                    // state = last_state;
                }
                cmd_pub.publish(cmd_vel);
            }
            else if(_state == 4)
            {
                if(ros::Time::now().toSec() - start_stamp.toSec() > 1)
                {
                    _state = -1;
                    start_stamp = ros::Time::now();
                    state = last_state;
                }
            }

        }

        ros::spinOnce();
        loop_rate.sleep();
    }
}